#include "MvCameraControl.h"
#include <camera_info_manager/camera_info_manager.hpp>
#include <image_transport/image_transport.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/utilities.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/image.hpp>

namespace hik_camera
{

struct CameraInstance {
  void* handle = nullptr;
  image_transport::CameraPublisher pub;
  sensor_msgs::msg::Image image_msg;
  sensor_msgs::msg::CameraInfo camera_info_msg;
  std::unique_ptr<camera_info_manager::CameraInfoManager> info_manager;
  std::string camera_name;
  MV_IMAGE_BASIC_INFO img_info{};
  MV_CC_PIXEL_CONVERT_PARAM convert_param{};
  int fail_count = 0;
  std::thread capture_thread;
};

class HikCameraNode : public rclcpp::Node
{
public:
  explicit HikCameraNode(const rclcpp::NodeOptions & options) : Node("hik_camera", options)
  {
    RCLCPP_INFO(this->get_logger(), "Starting HikCameraNode!");

    // 枚举所有USB相机设备
    MV_CC_DEVICE_INFO_LIST device_list{};
    MV_CC_EnumDevices(MV_USB_DEVICE, &device_list);
    RCLCPP_INFO(this->get_logger(), "Found camera count = %d", device_list.nDeviceNum);

    // 检查设备数量
    while (device_list.nDeviceNum < 2 && rclcpp::ok()) {
        RCLCPP_ERROR(this->get_logger(), "Need at least 2 cameras!");
        std::this_thread::sleep_for(std::chrono::seconds(1));
        MV_CC_EnumDevices(MV_USB_DEVICE, &device_list);
    }

    // 获取设备序列号并排序
    std::vector<std::pair<std::string, int>> camera_devices; // (serial_number, index)
    for (unsigned int i= 0; i < device_list.nDeviceNum; ++i) {
        std::string serial_number(reinterpret_cast<char*>(
            device_list.pDeviceInfo[i]->SpecialInfo.stUsb3VInfo.chSerialNumber));
        camera_devices.emplace_back(serial_number, i);
    }

    // 按序列号排序：DA0747367 -> camera1, 00F78889067 -> camera2
    std::sort(camera_devices.begin(), camera_devices.end(),
        [](const auto& a, const auto& b) {
            // DA0747367 应该排在前面（camera1）
            if (a.first == "DA0747367") return true;
            if (b.first == "DA0747367") return false;
            return a.first < b.first;
        });
    bool use_sensor_data_qos = this->declare_parameter("use_sensor_data_qos", true);
    // 初始化两个相机实例
    for (int i = 0; i < 2; ++i) {
     int device_index = camera_devices[i].second;
      CameraInstance cam;
      MV_CC_CreateHandle(&cam.handle, device_list.pDeviceInfo[device_index]);
      MV_CC_OpenDevice(cam.handle);

      // 获取相机信息
      MV_CC_GetImageInfo(cam.handle, &cam.img_info);
      cam.image_msg.data.reserve(cam.img_info.nHeightMax * cam.img_info.nWidthMax * 3);

      // 初始化转换参数
      cam.convert_param.nWidth = cam.img_info.nWidthValue;
      cam.convert_param.nHeight = cam.img_info.nHeightValue;
      cam.convert_param.enDstPixelType = PixelType_Gvsp_RGB8_Packed;

      /*
      // 创建发布器
      auto qos = use_sensor_data_qos ? rmw_qos_profile_sensor_data : rmw_qos_profile_default;
      cam.camera_name = "camera" + std::to_string(i+1);
      cam.pub = image_transport::create_camera_publisher(this, cam.camera_name + "/image_raw", qos);*/

      // 选择默认或 sensor_data 配置
        rmw_qos_profile_t custom_qos = use_sensor_data_qos
          ? rmw_qos_profile_sensor_data
          : rmw_qos_profile_default;

        // 切换到 KeepLast 历史模式，深度设为 10
        custom_qos.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST;
        custom_qos.depth   = 5;

        // 如果丢帧敏感，可继续使用 BestEffort
        custom_qos.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
        cam.camera_name = "camera" + std::to_string(i+1);

        cam.pub = image_transport::create_camera_publisher(this, cam.camera_name + "/image_raw", custom_qos);

      // 加载相机标定信息
      cam.info_manager = std::make_unique<camera_info_manager::CameraInfoManager>(this, cam.camera_name);
      std::string camera_info_url = this->declare_parameter(
          cam.camera_name + ".camera_info_url", 
          "package://hik_camera/config/" + cam.camera_name + "_info.yaml");
      if (cam.info_manager->validateURL(camera_info_url)) {
        cam.info_manager->loadCameraInfo(camera_info_url);
        cam.camera_info_msg = cam.info_manager->getCameraInfo();
      }

      // 声明相机特有参数
      declareCameraParameters(cam, i+1);
      cameras_.push_back(std::move(cam));
    }

    // 参数回调
    params_callback_handle_ = this->add_on_set_parameters_callback(
      [this](const std::vector<rclcpp::Parameter> & params) {
        return parametersCallback(params);
      });

    // 启动采集线程
    for (auto& cam : cameras_) {
      cam.capture_thread = std::thread([this, &cam]() { captureThread(cam); });
    }
  }

  ~HikCameraNode() override {
    for (auto& cam : cameras_) {
      if (cam.capture_thread.joinable()) cam.capture_thread.join();
      if (cam.handle) {
        MV_CC_StopGrabbing(cam.handle);
        MV_CC_CloseDevice(cam.handle);
        MV_CC_DestroyHandle(&cam.handle);
      }
    }
    RCLCPP_INFO(this->get_logger(), "HikCameraNode destroyed!");
  }

private:
  void declareCameraParameters(CameraInstance& cam, int index) {
    std::string prefix = "camera" + std::to_string(index) + ".";

    // 曝光时间
    MVCC_FLOATVALUE exposure_val{};
    MV_CC_GetFloatValue(cam.handle, "ExposureTime", &exposure_val);
    double exposure = this->declare_parameter(prefix + "exposure_time", exposure_val.fCurValue);
    MV_CC_SetFloatValue(cam.handle, "ExposureTime", exposure);

    // 增益
    MVCC_FLOATVALUE gain_val{};
    MV_CC_GetFloatValue(cam.handle, "Gain", &gain_val);
    double gain = this->declare_parameter(prefix + "gain", gain_val.fCurValue);
    MV_CC_SetFloatValue(cam.handle, "Gain", gain);
  }

  rcl_interfaces::msg::SetParametersResult parametersCallback(
      const std::vector<rclcpp::Parameter>& parameters) 
  {
    rcl_interfaces::msg::SetParametersResult result;
    result.successful = true;

    for (const auto& param : parameters) {
      const std::string& name = param.get_name();
      size_t dot_pos = name.find('.');
      if (dot_pos == std::string::npos) continue;

      std::string cam_prefix = name.substr(0, dot_pos);
      std::string param_name = name.substr(dot_pos+1);

      for (auto& cam : cameras_) {
        if (cam.camera_name != cam_prefix) continue;

        if (param_name == "exposure_time") {
          int ret = MV_CC_SetFloatValue(cam.handle, "ExposureTime", param.as_double());
          if (ret != MV_OK) {
            result.successful = false;
            result.reason = "Failed to set exposure for " + cam.camera_name;
          }
        }
        else if (param_name == "gain") {
          int ret = MV_CC_SetFloatValue(cam.handle, "Gain", param.as_double());
          if (ret != MV_OK) {
            result.successful = false;
            result.reason = "Failed to set gain for " + cam.camera_name;
          }
        }
      }
    }
    return result;
  }

  void captureThread(CameraInstance& cam) {
    MV_FRAME_OUT out_frame{};
    cam.image_msg.header.frame_id = cam.camera_name + "_optical_frame";
    cam.image_msg.encoding = "rgb8";

    MV_CC_StartGrabbing(cam.handle);

    while (rclcpp::ok()) {
      int nRet = MV_CC_GetImageBuffer(cam.handle, &out_frame, 1000);
      if (nRet == MV_OK) {
        // 图像格式转换
        cam.convert_param.pDstBuffer = cam.image_msg.data.data();
        cam.convert_param.nDstBufferSize = cam.image_msg.data.size();
        cam.convert_param.pSrcData = out_frame.pBufAddr;
        cam.convert_param.nSrcDataLen = out_frame.stFrameInfo.nFrameLen;
        cam.convert_param.enSrcPixelType = out_frame.stFrameInfo.enPixelType;
        MV_CC_ConvertPixelType(cam.handle, &cam.convert_param);

        // 填充并发布消息
        cam.image_msg.header.stamp = now();
        cam.image_msg.height = out_frame.stFrameInfo.nHeight;
        cam.image_msg.width = out_frame.stFrameInfo.nWidth;
        cam.image_msg.step = cam.image_msg.width * 3;
        cam.image_msg.data.resize(cam.image_msg.step * cam.image_msg.height);

        cam.camera_info_msg.header = cam.image_msg.header;
        cam.pub.publish(cam.image_msg, cam.camera_info_msg);

        MV_CC_FreeImageBuffer(cam.handle, &out_frame);
        cam.fail_count = 0;
      } else {
        RCLCPP_WARN(get_logger(), "[%s] Get image failed: 0x%x", 
                   cam.camera_name.c_str(), nRet);
        if (++cam.fail_count > 5) {
          RCLCPP_FATAL(get_logger(), "[%s] Too many failures!", cam.camera_name.c_str());
          rclcpp::shutdown();
        }
      }
    }
  }

  std::vector<CameraInstance> cameras_;
  OnSetParametersCallbackHandle::SharedPtr params_callback_handle_;
};

}  // namespace hik_camera

#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(hik_camera::HikCameraNode)